#ifndef __JUMP_POINT_SEARCH_H__
#define __JUMP_POINT_SEARCH_H__

#include <mc_path_planner/planner_base.h>
#include <mc_path_planner/graph_planner/node_2d.h>

#include <queue>
#include <unordered_map>
#include <vector>
#include <algorithm>  // for std::reverse
using namespace std;

class JPSPlanner : public PlannerBase {
    public:
        JPSPlanner(boost::shared_ptr<MapBase> map_handler_ptr);
        ~JPSPlanner();

        // 路径规划
        mc_status pathGenerate(Point2i start, Point2i goal, vector<Point2i>& points);

        // 检测跳点
        Node2DPtr JumpPoint(Node2DPtr node, int dir_x, int dir_y);
        // 检测强制邻居
        bool detectForceNeighbor(Node2DPtr node, int dir_x, int dir_y);

    private:
        ros::NodeHandle nh_;
        ros::Publisher debugMapPub_;
        nav_msgs::OccupancyGrid debugGrid_;
        bool debug_ = false;

        Node2D nodeStart_, nodeGoal_;

};

#endif